﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace XRoute.PathFinding
{
    /// <summary>
    /// A simple priority queue, that keeps elements sorted by a priority which does not
    /// have to be unique. The dequeue operation will always get the element with the the
    /// lowest priority value.
    /// </summary>
    /// <typeparam name="TPriority">The type of the priority.</typeparam>
    /// <typeparam name="TValue">The type of the value.</typeparam>
    class PriorityQueue<TPriority, TValue> where TPriority : IComparable<TPriority>
    {
        ///////////////////////////////////////////////////////////////////////

        readonly SortedDictionary<TPriority, Queue<TValue>> _list = new SortedDictionary<TPriority, Queue<TValue>>();


        ///////////////////////////////////////////////////////////////////////

        public bool IsEmpty
        {
            get { return _list.Any() == false; }
        }

        public void Enqueue(TPriority priority, TValue value)
        {
            Queue<TValue> queue;

            if (_list.TryGetValue(priority, out queue) == false)
            {
                queue = new Queue<TValue>();
                _list.Add(priority, queue);
            }

            queue.Enqueue(value);
        }

        public TValue Dequeue()
        {
            // will throw if there isn’t any first element!
            var pair = _list.First();
            var value = pair.Value.Dequeue();

            if (pair.Value.Count == 0) // nothing left of the top priority.
                _list.Remove(pair.Key);

            return value;
        }
    }
}
